package de.akob.physics;

import java.util.ArrayList;

import de.akob.mathematics.MathVector;
import de.akob.mathematics.Mathematics;

/**
 * Physic engine that handles movement and collision detection. Currently there is only support for rigid bodies.
 */
public class PhysicsEngine {
	/**
	 * Pair of objects with potential collision.
	 */
	private class CollisionPair {
		// bodies with potential collision
		private PhysicalObject body1, body2;
		
		/**
		 * Constructs a collision pair for the given bodies.
		 * 
		 * @param body1 first body
		 * @param body2 second body
		 */
		public CollisionPair(PhysicalObject body1, PhysicalObject body2) {
			this.body1 = body1;
			this.body2 = body2;
		}
		
		/**
		 * Processes the collision (if any) represented by this collision pair.
		 */
		public void processCollision() {
			// // collision handling for two spheres
			// if (body1.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_SPERE
			// && body2.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_BOX) {
			//				
			// }
			// body1.applyMomentum(new MathVector((new Random()).nextDouble()/100., (new Random()).nextDouble()/100.,
			// (new Random()).nextDouble()/100.));
			// body2.applyMomentum(new MathVector((new Random()).nextDouble()/100., (new Random()).nextDouble()/100.,
			// (new Random()).nextDouble()/100.));
		}
	}
	
	/**
	 * Checks whether the bounding containers of the given objects do intersect.
	 * 
	 * @param body1 first body
	 * @param body2 second body
	 * @return whether the bounding containers of the given objects do intersect
	 */
	public static boolean checkMacroCollision(PhysicalObject body1, PhysicalObject body2) {
		if (body1.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_BOX
		        && body2.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_BOX) {
			double x1 = body1.getPosition().get(1), x2 = body2.getPosition().get(1);
			double y1 = body1.getPosition().get(2), y2 = body2.getPosition().get(2);
			double z1 = body1.getPosition().get(3), z2 = body2.getPosition().get(3);
			if (body1.xMax + x1 < body2.xMin + x2 || body1.xMin + x1 > body2.xMax + x2
			        || body1.yMax + y1 < body2.yMin + y2 || body1.yMin + y1 > body2.yMax + y2
			        || body1.zMax + z1 < body2.zMin + z2 || body1.zMin + z1 > body2.zMax + z2)
				return false;
			else
				return true;
		} else if (body1.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_SPHERE
		        && body2.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_SPHERE) {
			if (body1.radius + body2.radius < Mathematics.distance(body1.getPosition(), body2.getPosition()))
				return false;
			else
				return true;
		} else if (body1.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_BOX
		        && body2.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_SPHERE) {
			// for simplicity, just check for intersection of enclosing cube of sphere and bounding box
			double x1 = body1.getPosition().get(1), x2 = body2.getPosition().get(1);
			double y1 = body1.getPosition().get(2), y2 = body2.getPosition().get(2);
			double z1 = body1.getPosition().get(3), z2 = body2.getPosition().get(3);
			if (body1.xMax + x1 < x2 - body2.radius || body1.xMin + x1 > x2 + body2.radius
			        || body1.yMax + y1 < y2 - body2.radius || body1.yMin + y1 > y2 + body2.radius
			        || body1.zMax + z1 < z2 - body2.radius || body1.zMin + z1 > z2 + body2.radius)
				return false;
			else
				return true;
		} else if (body1.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_SPHERE
		        && body2.boundingContainer == PhysicalObject.BoundingContainer.BOUNDING_BOX) {
			// for simplicity, just check for intersection of enclosing cube of sphere and bounding box
			double x1 = body1.getPosition().get(1), x2 = body2.getPosition().get(1);
			double y1 = body1.getPosition().get(2), y2 = body2.getPosition().get(2);
			double z1 = body1.getPosition().get(3), z2 = body2.getPosition().get(3);
			if (body2.xMax + x2 < x1 - body1.radius || body2.xMin + x2 > x1 + body1.radius
			        || body2.yMax + y2 < y1 - body1.radius || body2.yMin + y2 > y1 + body1.radius
			        || body2.zMax + z2 < z1 - body1.radius || body2.zMin + z2 > z1 + body1.radius)
				return false;
			else
				return true;
		}
		
		System.err.println("Warning: unknown pair of bounding containers (abort collision detection for this pair)");
		return false;
	}
	
	// list of bodies in system
	private ArrayList<PhysicalObject> bodies = new ArrayList<PhysicalObject>();
	
	/**
	 * Adds the given body to the system.
	 * 
	 * @param body body to be added
	 */
	public void addBody(PhysicalObject body) {
		bodies.add(body);
	}
	
	/**
	 * Collision handling.
	 * 
	 * @param dt elapsed time (in seconds)
	 */
	public void handleCollisions(double dt) {
		// temporarily update body states (velocity and position)
		ArrayList<MathVector> backup_positions = new ArrayList<MathVector>();
		ArrayList<MathVector> backup_velocities = new ArrayList<MathVector>();
		for (PhysicalObject body : bodies) {
			backup_positions.add(body.getPosition().clone());
			backup_velocities.add(body.getVelocity().clone());
			body.updateVelocity(dt);
			body.updatePosition(dt);
		}
		
		// get (potential) collision pairs
		ArrayList<CollisionPair> collisionPairs = new ArrayList<CollisionPair>();
		for (int i = 0; i < bodies.size(); i++) {
			for (int j = i + 1; j < bodies.size(); j++) {
				PhysicalObject body1 = bodies.get(i);
				PhysicalObject body2 = bodies.get(j);
				if (checkMacroCollision(body1, body2) == true)
					collisionPairs.add(new CollisionPair(body1, body2));
			}
		}
		
		// restore original body states (velocity and position)
		for (int i = 0; i < bodies.size(); i++) {
			bodies.get(i).setPosition(backup_positions.get(i));
			bodies.get(i).setVelocity(backup_velocities.get(i));
		}
		
		// process collision pairs
		for (CollisionPair collisionPair : collisionPairs)
			collisionPair.processCollision();
	}
	
	/**
	 * Contact handling
	 * 
	 * @param dt elapsed time (in seconds)
	 */
	public void handleContacts(double dt) {
		// TODO
	}
	
	/**
	 * Updates the system using the given elapsed time <code>dt</code> as the time step.
	 * 
	 * @param dt elapsed time (in seconds)
	 */
	public void update(double dt) {
		handleCollisions(dt);
		updateVelocities(dt);
		handleContacts(dt);
		updatePositions(dt);
	}
	
	/**
	 * Updates the velocities of all bodies in the system.
	 * 
	 * @param dt (in seconds)
	 */
	public void updatePositions(double dt) {
		for (PhysicalObject body : bodies)
			body.updatePosition(dt);
	}
	
	/**
	 * Updates the velocities of all bodies in the system.
	 * 
	 * @param dt (in seconds)
	 */
	public void updateVelocities(double dt) {
		for (PhysicalObject body : bodies)
			body.updateVelocity(dt);
	}
}
